Observations in the car coordinate system can be transformed into map coordinates ( \text{x}_m and \text{y}_m ) by passing car observation coordinates ( \text{x}_c and \text{y}_c ), map particle coordinates ( \text{x}_p and \text{y}_p ), and our rotation angle (-90 degrees) through a homogenous transformation matrix. This homogenous transformation matrix, shown below, performs rotation and translation.

Homogenous Transformation

Matrix multiplication results in:

\text{x}_m= \text{x}_p + (\cos\theta \times \text{x}_c) - (\sin\theta \times \text{y}_c)

\text{y}_m= \text{y}_p + (\sin\theta \times \text{x}_c) + (\cos\theta \times \text{y}_c)

Quiz Solutions

Observation 1 Solution

#include <cmath>
#include <iostream>

int main() {
  // define coordinates and theta
  double x_part, y_part, x_obs, y_obs, theta;
  x_part = 4;
  y_part = 5;
  x_obs = 2;
  y_obs = 2;
  theta = -M_PI/2; // -90 degrees

  // transform to map x coordinate
  double x_map;
  x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);

  // transform to map y coordinate
  double y_map;
  y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);

  // (6,3)
  std::cout << int(round(x_map)) << ", " << int(round((y_map)) << std::endl;

  return 0;
}

Observation 2 Solution

#include <cmath>
#include <iostream>

int main() {
  // define coordinates and theta
  double x_part, y_part, x_obs, y_obs, theta;
  x_part = 4;
  y_part = 5;
  x_obs = 3;
  y_obs = -2;
  theta = -M_PI/2; // -90 degrees

  // transform to map x coordinate
  double x_map;
  x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);

  // transform to map y coordinate
  double y_map;
  y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);

  // (2,2)
  std::cout << int(round(x_map)) << ", " << int(round(y_map)) << std::endl;

  return 0;
}

Observation 3 Solution

#include <cmath>
#include <iostream>

int main() {
  // define coordinates and theta
  double x_part, y_part, x_obs, y_obs, theta;
  x_part = 4;
  y_part = 5;
  x_obs = 0;
  y_obs = -4;
  theta = -M_PI/2; // -90 degrees

  // transform to map x coordinate
  double x_map;
  x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);

  // transform to map y coordinate
  double y_map;
  y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);

  // (0,5)
  std::cout << int(round(x_map)) << ", " << int(round(y_map)) << std::endl;

  return 0;
}